#include "LQRController.h"
#include "string.h"
// // 系统矩阵
// const float A[3][3] = {{0, 1, 0}, {0, 0, 1}, {0, 0, 0}};
// const float B[3] = {0, 0, 1.0};
// // 输出矩阵 (假设只能测量角度和角速度)
// const float C[2][3] = {{1, 0, 0}, {0, 1, 0}};
// // LQR反馈增益矩阵 (假设已经计算好)
// float K[3] = {1.0, 1.0, 1.0};
// // 状态观测器增益矩阵 (假设已经计算好)
// float L[3][2] = {{0.5, 0.5}, {1.0, 1.0}, {0.1, 0.1}};
LQRController::LQRController(float *k_buff, float (*l_buff)[3][2], float (*a_buff)[3][3], float *b_buff, float (*c_buff)[2][3], float T)
{
    dt = T;
    // A_ptr = a_buff;
    // A_ptr = a_buff;
    memcpy(A_ptr, a_buff, sizeof(A_ptr));
    B_ptr = b_buff;
    // C_ptr = c_buff;
    memcpy(C_ptr, c_buff, sizeof(C_ptr));
    K_ptr = k_buff;
    // L_ptr = l_buff;
    memcpy(L_ptr, l_buff, sizeof(L_ptr));
}

// LQR控制器(输入期望值)
float LQRController::lqr_control(float setval[3])
{
    // 定义误差
    float error[3] = { setval[0] - current_state[0], setval[1] - current_state[1], setval[2] - current_state[2]} ;
    // 计算控制输入 u = -K * x
    u = -(K_ptr[0] * error[0] + K_ptr[1] * error[1] + K_ptr[2] * error[2]);
    return u;
}

// 状态观测器更新
void LQRController::update_observer()
{
    // 观测器状态更新方程
    float z[3] = {current_state[0], current_state[1], current_state[2]};
    float z_next[3] = {0};

    // 状态更新
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            z_next[i] +=( A_ptr[i][j] ) * z[j];
        }
        z_next[i] += B_ptr[i] * u;
    }

    // 观测器误差校正
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            z_next[i] += L_ptr[i][j] * (current_state[j] - (C_ptr[j][0] * z[0] + C_ptr[j][1] * z[1] + C_ptr[j][2] * z[2]));
        }
    }

    // 更新状态(只观测角加速度)
    // state->angle = z_next[0];
    // state->angular_velocity = z_next[1];
    current_state[2] = z_next[2];
}


